#include "util.h"

/* Function prototypes */

static void merge_c(data_t *A, int p, int q, int r);
static void copy_c(data_t * source, data_t * dest, int n) ;

/* Function definitions */

/* Basic merge sort */
void sort_c(data_t *A, int p, int r) 
{
	assert (A) ;
	assert (p >= 0);
	assert (r >= 0);
	if (p < 0)
		return;
	if (r < 0)
		return;
	if (p < r)
	{
		if (r - p < 25) {
			isort(A+p, A+r);						
		} else {
			int q = (p + r) / 2 ;
			sort_c(A, p, q);
			sort_c(A, q + 1, r);
			merge_c(A, p, q, r) ;
		}
	}	
}

/* A merge routine. Merges the sub-arrays A [p..q] and A [q + 1..r].
 * Uses two arrays 'left' and 'right' in the merge operation.
 */
inline static void merge_c(data_t * __restrict A, int p, int q, int r) 
{ 
	assert(A) ;
	assert(p <= q) ;
	assert((q + 1) <= r) ;
	int n1 = q - p + 1;
	int n2 = r - q ;

	data_t * left = 0, * right = 0 ; 
	mem_alloc(&left, n1 + 1) ;
	mem_alloc(&right, n2 + 1) ;
        if (left == NULL || right == NULL)
        {
                mem_free (&left) ;
		mem_free (&right);
                return ;
        }
	
	copy_c (&(A [p]), left, n1) ;
	copy_c (&(A [q + 1]), right, n2) ; 
	left [n1] = UINT_MAX ;
	right [n2] = UINT_MAX ;

	data_t * __restrict lptr = left;
	data_t * __restrict rptr = right;

	data_t * end = A+r;
	A = A+p;

	while (A <= end) {
		data_t cmp = (*lptr <= *rptr);
		data_t min = *rptr ^ ((*rptr ^ *lptr) & (-cmp));
		*A++ = min;
		lptr += cmp;
		rptr += !cmp;
		
		/*
		 * It's normally fine if there is an integer of max size in the list.
		 * But if the left list reaches the end and there is a max size int still in the right list, it will increment the left instead.
		 * This checks for that and fixes the indices.
		 */
		if (lptr - left >= n1) {	
			lptr--;
			rptr++;
		}	
	}

	mem_free(&left) ;
	mem_free(&right);
}

inline static void copy_c(data_t * source, data_t * dest, int n) 
{                                                                             
        assert (dest) ;                                                       
        assert (source) ;                                                     
	data_t * sptr = source;
	data_t * dptr = dest;
	int i ;                                                               
        for (i = 0 ; i < n ; i++)                                             
        {                                                                     
		*(dptr + i) = *(sptr + i);
        }                                                                     
}
